﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;

namespace Game_Engine
{
    public class AStarSearch
    {
        public int Cols { get; set; }
        public int Rows { get; set; }
        public AStarNode[,] Nodes { get; set; }
        public AStarNode Start { get; set; }
        public AStarNode End { get; set; }
        public int ImpassableNodes { get; set; }

        private SortedDictionary<float, List<AStarNode>> openList;

        public AStarSearch(int rows, int cols)
        {
            Rows = rows;
            Cols = cols;
            Nodes = new AStarNode[Rows, Cols];
            for (int r = 0; r < Rows; r++)
                for(int c = 0; c < Cols; c++)
                    Nodes[r,c] = new AStarNode(c, r, new Vector3(c, 0, r));

            openList = new SortedDictionary<float, List<AStarNode>>();

        }

        public void Search()
        {
            //In the beginning, we must make the grid and be sure to reset necessary parameters
            openList.Clear();
            
            foreach(AStarNode node in Nodes)
            {
                node.Cost = Single.MaxValue;
                node.Closed = false;
                node.Parent = null;
                node.Heuristic = Vector3.Distance(node.Position, End.Position);
                //node.Heuristic = 0;
            }
            
            AddToOpenList(Start);

            while(openList.Count > 0)
            {
                //Get node with shortest cost from openList
                AStarNode node = GetBestNode();

                //If the node is the end node we are done
                if (node == End)
                    break;

                //Add neighbor to openList - check for array out of bounds exceptions
                if(node.Row < Rows - 1)
                    AddToOpenList(Nodes[node.Row + 1, node.Col], node);
                if(node.Row > 0)
                    AddToOpenList(Nodes[node.Row - 1, node.Col], node);
                if(node.Col < Cols - 1)
                    AddToOpenList(Nodes[node.Row, node.Col+1], node);
                if(node.Col > 0)
                    AddToOpenList(Nodes[node.Row, node.Col-1], node);
            }
        }

        private void AddToOpenList(AStarNode node, AStarNode parent = null)
        {
            if (!node.Passable || node.Closed)
                return;

            if (parent == null)
                node.Cost = 0;
            else
            {
                float cost = parent.Cost + 1;   //+1 cost is arbitrary and could be different computation
                if (node.Cost > cost)
                {
                    RemoveFromOpenList(node);
                    node.Cost = cost;
                    node.Parent = parent;
                }
                else               //Without a change in cost, we dare not add the
                    return;       //new node
            }
            if (!openList.ContainsKey(node.Cost+node.Heuristic))
                openList[node.Cost+node.Heuristic] = new List<AStarNode>();
            openList[node.Cost+node.Heuristic].Add(node);
        }

        private void RemoveFromOpenList(AStarNode node)
        {
            if (openList.ContainsKey(node.Cost+node.Heuristic))
            {
                openList[node.Cost+node.Heuristic].Remove(node);            //get er outta there!
                if (openList[node.Cost+node.Heuristic].Count == 0)          //And if that is the last element in the list, the list is now empty
                    openList.Remove(node.Cost);                     //Don't need no empty lists in openList, so remove its float key pointing to it via that 
            }                                                       //last node's associated cost.
        }

        private AStarNode GetBestNode()
        {
            AStarNode node = openList.ElementAt(0).Value[0];    //For a sorted dictionary we index by key, then access what data we need
            openList.ElementAt(0).Value.RemoveAt(0);            //When we got it, get er outta there!
            if (openList.ElementAt(0).Value.Count == 0)         //And if that is the last element in the list, the list is now empty
                openList.Remove(node.Cost+node.Heuristic);                     //Don't need no empty lists in openList, so remove its float key pointing to it via that 
            node.Closed = true;                                 //last node's associated cost.
            return node;
        }
    }
}
